
#include <stdio.h>

#include <util/mem.h>
#include <util/common_macros.h>
#include <util/msg.h>

#include "../imp_types.h"

#include "point.h"
#include "triangle.h"

#include "predicates.h"

static long int count = 0;
void imp_triangle_set_count(long int c) {
	count = c;
}
/* ========================================================================== */

static boolean __triangle_destroy(ImpObject * object) {
	ImpTriangle * t;
	t = IMP_TRIANGLE(object);

	imp_edge_remove_triangle(t->e[0], t);
	imp_edge_remove_triangle(t->e[1], t);
	imp_edge_remove_triangle(t->e[2], t);

	t->e[0] = NULL;
	t->e[1] = NULL;
	t->e[2] = NULL;
	
	return TRUE;

}
static void __triangle_class_init(ImpObjectClass * klass) {
	klass->object_clone = NULL;
	klass->object_destroy = __triangle_destroy;
	klass->object_attributes = NULL;
}
static void __triangle_init(ImpObject * object) {
	ImpTriangle * t;
	t = IMP_TRIANGLE(object);

	t->e[0] = NULL;
	t->e[1] = NULL;
	t->e[2] = NULL;

}
/* ========================================================================== */

ImpTriangleClass * imp_triangle_get_class(void) {
	static ImpTriangleClass * klass = NULL;

	if (klass == NULL) {
		ImpObjectClassInfo triangle_info = {
											"ImpTriangle",
											sizeof (ImpTriangle),
											sizeof (ImpTriangleClass),
											(ImpObjectClassInitFunc) __triangle_class_init,
											(ImpObjectInitFunc) __triangle_init,
											(ImpArgSetFunc) NULL,
											(ImpArgGetFunc) NULL
		};

		ImpObjectClass * parent_class = imp_object_get_class();
		ImpObjectClass * triangle_class = imp_object_class_new(parent_class, &triangle_info);
		klass = IMP_TRIANGLE_CLASS(triangle_class);
	}
	return klass;
}
ImpTriangle * imp_triangle_new(ImpEdge * e1, ImpEdge * e2, ImpEdge * e3) {
	ImpTriangle * t;

	t = IMP_TRIANGLE(imp_object_new(IMP_OBJECT_CLASS(imp_triangle_get_class())));

	imp_triangle_set(t, e1, e2, e3);

#ifdef IMP_DEBUG
	printf("New t = ");
	imp_triangle_print(t);
	printf("\n");
#endif
	return t;
}
void imp_triangle_set(ImpTriangle * t, ImpEdge * e1, ImpEdge * e2, ImpEdge * e3) {

	_exit_if_fail(e1 && e2 && e3);

	if (e1 == e2) {
		msg_warning("Ooops!\n");
	}
	if (e2 == e3) {
		msg_warning("Ooops!\n");
	}
	if (e3 == e1) {
		msg_warning("Ooops!\n");
	}
	if (!imp_segment_are_connected(IMP_SEGMENT(e1), IMP_SEGMENT(e2))) {
		msg_warning("Ooops!\n");
	}
	if (!imp_segment_are_connected(IMP_SEGMENT(e2), IMP_SEGMENT(e3))) {
		msg_warning("Ooops!\n");
	}
	if (!imp_segment_are_connected(IMP_SEGMENT(e3), IMP_SEGMENT(e1))) {
		msg_warning("Ooops!\n");
	}

	t->e[0] = e1;
	t->e[1] = e2;
	t->e[2] = e3;
	
	//t->color = 0;
	//t->visited = FALSE;

	imp_edge_add_triangle(t->e[0], t);
	imp_edge_add_triangle(t->e[1], t);
	imp_edge_add_triangle(t->e[2], t);
	
	t->id = ++count;
}
/* ========================================================================== */

void imp_triangle_extract(ImpTriangle * t,
						  ImpEdge * e,
						  ImpVertex ** v1,
						  ImpVertex ** v2,
						  ImpVertex ** v3,
						  ImpEdge ** e1,
						  ImpEdge ** e2,
						  ImpEdge ** e3) {

	ImpVertex *vv1, *vv2, *vv3;
	ImpEdge *ee1, *ee2, *ee3;

	imp_triangle_extract_vertices_and_edges(t, &vv1, &vv2, &vv3, &ee1, &ee2, &ee3);

	if (e == NULL || e == ee1) {
		*e1 = ee1;
		*e2 = ee2;
		*e3 = ee3;

		*v1 = vv1;
		*v2 = vv2;
		*v3 = vv3;

	} else if (e == ee2) {
		*e1 = ee2;
		*e2 = ee3;
		*e3 = ee1;

		*v1 = vv2;
		*v2 = vv3;
		*v3 = vv1;

	} else if (e == ee3) {
		*e1 = ee3;
		*e2 = ee1;
		*e3 = ee2;

		*v1 = vv3;
		*v2 = vv1;
		*v3 = vv2;

	} else {
		msg_error("Ooops!\n");
	}


}
void imp_triangle_extract_vertices_ord(ImpTriangle * t, ImpVertex * v, ImpVertex ** v1, ImpVertex ** v2, ImpVertex ** v3) {

	ImpVertex *vv1, *vv2, *vv3;

	imp_triangle_extract_vertices(t, &vv1, &vv2, &vv3);

	if (v == NULL || v == vv1) {
		*v1 = vv1;
		*v2 = vv2;
		*v3 = vv3;
	} else if (v == vv2) {
		*v1 = vv2;
		*v2 = vv3;
		*v3 = vv1;
	} else if (v == vv3) {
		*v1 = vv3;
		*v2 = vv1;
		*v3 = vv2;
	} else {
		msg_error("Ooops!\n");
	}


}
void imp_triangle_extract_vertices(ImpTriangle * t,
								   ImpVertex ** v1,
								   ImpVertex ** v2,
								   ImpVertex ** v3) {

	ImpVertex * vt;
	ImpSegment * s1;
	ImpSegment * s2;
	ImpSegment * s3;

	ImpVertex *vv1, *vv2, *vv3;

	s1 = IMP_SEGMENT(t->e[0]);
	s2 = IMP_SEGMENT(t->e[1]);
	s3 = IMP_SEGMENT(t->e[2]);

	vv1 = s1->v[0];
	vv2 = s1->v[1];

	/*
		if (vv1 == vv2) {
			msg_error("Ooops!\n");
		}
	 */

	if (imp_segment_have_vertex(s2, vv1)) {
		if (s2->v[0] == vv1) {
			vv3 = s2->v[1];
		} else if (s2->v[1] == vv1) {
			vv3 = s2->v[0];
		} else {
			msg_error("Ooops!\n");
		}
	} else if (imp_segment_have_vertex(s3, vv1)) {
		if (s3->v[0] == vv1) {
			vv3 = s3->v[1];
		} else if (s3->v[1] == vv1) {
			vv3 = s3->v[0];
		} else {
			msg_error("Ooops!\n");
		}
	} else {
		msg_error("Ooops!\n");
	}

	/* reordering */
	if (imp_point_in_left(IMP_POINT(vv1), IMP_POINT(vv2), IMP_POINT(vv3))) {
		vt = vv1;
		vv1 = vv2;
		vv2 = vt;
	}
	/*
		if (imp_point_in_left(IMP_POINT(vv1), IMP_POINT(vv2), IMP_POINT(vv3))) {
			msg_error("Ooops!\n");
		}

		if (vv1 == vv2) {
			msg_error("Ooops!\n");
		}
		if (vv2 == vv3) {
			msg_error("Ooops!\n");
		}
		if (vv3 == vv1) {
			msg_error("Ooops!\n");
		}
	 */

	*v1 = vv1;
	*v2 = vv2;
	*v3 = vv3;
}
void imp_triangle_extract_vertices_and_edges(
											 ImpTriangle * t,
											 ImpVertex ** v1,
											 ImpVertex ** v2,
											 ImpVertex ** v3,
											 ImpEdge ** e1,
											 ImpEdge ** e2,
											 ImpEdge ** e3) {


	ImpVertex *vv1, *vv2, *vv3;
	ImpEdge *ee1, *ee2, *ee3;

	ImpSegment *s1, *s2, *s3;
	s1 = IMP_SEGMENT(t->e[0]);
	s2 = IMP_SEGMENT(t->e[1]);
	s3 = IMP_SEGMENT(t->e[2]);

	*v1 = *v2 = *v3 = NULL;
	*e1 = *e2 = *e3 = NULL;

	/* Extracting the three different vertices */
	imp_triangle_extract_vertices(t, &vv1, &vv2, &vv3);

	/* = Extracting the edges =============================================== */
	if (imp_segment_connect(s1, vv1, vv2)) {
		ee1 = t->e[0];
	} else if (imp_segment_connect(s1, vv2, vv3)) {
		ee2 = t->e[0];
	} else if (imp_segment_connect(s1, vv3, vv1)) {
		ee3 = t->e[0];
	} else {
		msg_error("Ooops!\n");
	}

	if (imp_segment_connect(s2, vv1, vv2)) {
		ee1 = t->e[1];
	} else if (imp_segment_connect(s2, vv2, vv3)) {
		ee2 = t->e[1];
	} else if (imp_segment_connect(s2, vv3, vv1)) {
		ee3 = t->e[1];
	} else {
		msg_error("Ooops!\n");
	}

	if (imp_segment_connect(s3, vv1, vv2)) {
		ee1 = t->e[2];
	} else if (imp_segment_connect(s3, vv2, vv3)) {
		ee2 = t->e[2];
	} else if (imp_segment_connect(s3, vv3, vv1)) {
		ee3 = t->e[2];
	} else {
		msg_error("Ooops!\n");
	}

	if (ee1 == NULL || ee2 == NULL || ee3 == NULL) {
		msg_error("Ooops!\n");
	}

	*v1 = vv1;
	*v2 = vv2;
	*v3 = vv3;

	*e1 = ee1;
	*e2 = ee2;
	*e3 = ee3;
	return;







	if (imp_segment_connect(IMP_SEGMENT(t->e[0]), *v1, *v2)) {
		*e1 = t->e[0];
	} else if (imp_segment_connect(IMP_SEGMENT(t->e[0]), *v2, *v3)) {
		*e2 = t->e[0];
	} else if (imp_segment_connect(IMP_SEGMENT(t->e[0]), *v3, *v1)) {
		*e3 = t->e[0];
	}

	if (imp_segment_connect(IMP_SEGMENT(t->e[1]), *v1, *v2)) {
		*e1 = t->e[1];
	} else if (imp_segment_connect(IMP_SEGMENT(t->e[1]), *v2, *v3)) {
		*e2 = t->e[1];
	} else if (imp_segment_connect(IMP_SEGMENT(t->e[1]), *v3, *v1)) {
		*e3 = t->e[1];
	}

	if (imp_segment_connect(IMP_SEGMENT(t->e[2]), *v1, *v2)) {
		*e1 = t->e[2];
	} else if (imp_segment_connect(IMP_SEGMENT(t->e[2]), *v2, *v3)) {
		*e2 = t->e[2];
	} else if (imp_segment_connect(IMP_SEGMENT(t->e[2]), *v3, *v1)) {
		*e3 = t->e[2];
	}



}
/* ========================================================================== */

boolean imp_triangle_have_vertex(ImpTriangle * t, ImpVertex * v) {

	ImpVertex *v1, *v2, *v3;
	ImpSegment *s1, *s2, *s3;

	s1 = IMP_SEGMENT(t->e[0]);
	s2 = IMP_SEGMENT(t->e[1]);
	s3 = IMP_SEGMENT(t->e[2]);

	imp_triangle_extract_vertices(t, &v1, &v2, &v3);

	/* Extracting the three different vertices */
	v1 = s1->v[0];
	v2 = s1->v[1];
	if (v1 == s2->v[0]) {
		v3 = s2->v[1];
	} else if (v1 == s2->v[1]) {
		v3 = s2->v[0];
	} else if (v1 == s3->v[0]) {
		v3 = s3->v[1];
	} else if (v1 == s3->v[1]) {
		v3 = s3->v[0];
	} else {
		msg_error("Oops!\n");
	}
	return (v == v1 || v == v2 || v == v3);
}
/*
boolean __imp_triangle_have_edge(ImpTriangle * t, ImpEdge * e) {
	return (t->e[0] == e || t->e[1] == e || t->e[2] == e);
}
 */

ImpVertex * imp_triangle_get_opposite_vertex(ImpTriangle * t, ImpEdge * e) {

	ImpVertex * v;
	//boolean r;

	_return_val_if_fail(t, NULL);
	_return_val_if_fail(e, NULL);

	if (t->e[0] == e) {
		v = IMP_SEGMENT(t->e[1])->v[0];
		if (v != IMP_SEGMENT(e)->v[0] && v != IMP_SEGMENT(e)->v[1])
			return v;
		return IMP_SEGMENT(t->e[1])->v[1];
	}
	if (t->e[1] == e) {
		v = IMP_SEGMENT(t->e[0])->v[0];
		if (v != IMP_SEGMENT(e)->v[0] && v != IMP_SEGMENT(e)->v[1])
			return v;
		return IMP_SEGMENT(t->e[0])->v[1];
	}
	if (t->e[2] == e) {
		v = IMP_SEGMENT(t->e[1])->v[0];
		if (v != IMP_SEGMENT(e)->v[0] && v != IMP_SEGMENT(e)->v[1])
			return v;
		return IMP_SEGMENT(t->e[1])->v[1];
	}

#ifdef IMP_DEBUG
	printf("t = ");
	imp_triangle_print(t);
	printf("\ne = ");
	imp_edge_print(e);
	printf("\n");
#endif

	msg_warning("Oops!\n");

	return NULL;
}
ImpEdge * imp_triangle_get_opposite_edge(ImpTriangle * t, ImpVertex * v) {

	boolean e1, e2, e3;

	_return_val_if_fail(t, NULL);
	_return_val_if_fail(v, NULL);

	e1 = imp_edge_have_vertex(t->e[0], v);
	e2 = imp_edge_have_vertex(t->e[1], v);
	e3 = imp_edge_have_vertex(t->e[2], v);

	if ((!e1) & e2 & e3) return t->e[0];
	if ((e1 & !e2) & e3) return t->e[1];
	if ((e1 & e2) & !e3) return t->e[2];

	msg_warning("Oops!\n");
	return NULL;
}
ImpTriangle * imp_triangle_get_opposite_triangle(ImpTriangle * t, ImpEdge * e) {

	ImpTriangle * topp;

	_return_val_if_fail(t && e, NULL);
	_return_val_if_fail(imp_triangle_have_edge(t, e), NULL);

	topp = IMP_TRIANGLE(e->faces->first->data);

	if (t == topp) {
		if (e->faces->first->next) return IMP_TRIANGLE(e->faces->first->next->data);
		else return NULL;
	} else return topp;

	msg_error("Oops!\n");
	return NULL;
}
ImpEdge * imp_triangle_get_shared_edge(ImpTriangle *T1, ImpTriangle *T2) {
	if (T1->e[0] == T2->e[0] || T1->e[0] == T2->e[1] || T1->e[0] == T2->e[2]) {
		return T1->e[0];
	}
	if (T1->e[1] == T2->e[0] || T1->e[1] == T2->e[1] || T1->e[1] == T2->e[2]) {
		return T1->e[1];
	}
	if (T1->e[2] == T2->e[0] || T1->e[2] == T2->e[1] || T1->e[2] == T2->e[2]) {
		return T1->e[2];
	}
	return NULL;
}
ImpTriangle * imp_triangle_get_neighbor(ImpTriangle * T, ImpEdge * e) {

	if (!imp_triangle_have_edge(T, e)) return NULL;

	if (e->faces->size == 1) return NULL;

	if (e->faces->first->data == T) {
		return e->faces->first->next->data;
	} else {
		return e->faces->first->data;
	}

	return NULL;
}
ImpEdge * imp_triangle_get_edge_that_connects_vertices(ImpTriangle * T, ImpVertex * v1, ImpVertex * v2) {
	if (imp_segment_connect(IMP_SEGMENT(T->e[0]), v1, v2)) return T->e[0];
	else if (imp_segment_connect(IMP_SEGMENT(T->e[1]), v1, v2)) return T->e[1];
	else if (imp_segment_connect(IMP_SEGMENT(T->e[2]), v1, v2)) return T->e[2];
	else return NULL;

}
real imp_triangle_get_area(ImpTriangle * T) {

	Ref2 p[3];
	real pa[2], pb[2], pc[2];

	// = Extract triangle points ===============================================
	ImpVertex * v1, * v2, * v3;
	imp_triangle_extract_vertices(T, &v1, &v2, &v3);

	pa[0] = IMP_POINT(v1)->x;
	pa[1] = IMP_POINT(v1)->y;

	pb[0] = IMP_POINT(v2)->x;
	pb[1] = IMP_POINT(v2)->y;

	pc[0] = IMP_POINT(v3)->x;
	pc[1] = IMP_POINT(v3)->y;

	return _area(pa, pb, pc);
}
/* ========================================================================== */

ImpPoint imp_triangle_barycenter(ImpTriangle * t) {

	ImpPoint p;
	ImpVertex *v1, *v2, *v3;
	real x, y, z;

	imp_triangle_extract_vertices(t, &v1, &v2, &v3);

	x = (IMP_POINT(v1)->x + IMP_POINT(v2)->x + IMP_POINT(v3)->x) / 3.;
	y = (IMP_POINT(v1)->y + IMP_POINT(v2)->y + IMP_POINT(v3)->y) / 3.;
	z = (IMP_POINT(v1)->z + IMP_POINT(v2)->z + IMP_POINT(v3)->z) / 3.;

	imp_point_set(&p, x, y, z);

	return p;
}
ImpPoint * imp_triangle_circumcircle_center(ImpTriangle * t) {
	ImpVertex * v1, * v2, * v3;

	real xa, ya, xb, yb, xc, yc;
	real xd, yd, xe, ye;
	real xad, yad, xae, yae;
	real det;

	real x, y;

	_return_val_if_fail(t != NULL, NULL);

	imp_triangle_extract_vertices(t, &v1, &v2, &v3);

	xa = IMP_POINT(v1)->x;
	ya = IMP_POINT(v1)->y;
	xb = IMP_POINT(v2)->x;
	yb = IMP_POINT(v2)->y;
	xc = IMP_POINT(v3)->x;
	yc = IMP_POINT(v3)->y;
	xd = (xa + xb) / 2.;
	yd = (ya + yb) / 2.;
	xe = (xa + xc) / 2.;
	ye = (ya + yc) / 2.;
	xad = xd - xa;
	yad = yd - ya;
	xae = xe - xa;
	yae = ye - ya;
	det = xad * yae - xae*yad;

	if (det == 0.) return NULL;

	x = (yae * yad * (yd - ye) + xad * yae * xd - xae * yad * xe) / det;
	y = -(xae * xad * (xd - xe) + yad * xae * yd - yae * xad * ye) / det;

	return imp_point_new(x, y, 0.0);

	/*
		return imp_point_new((yae * yad * (yd - ye) + xad * yae * xd - xae * yad * xe) / det,
				-(xae * xad * (xd - xe) + yad * xae * yd - yae * xad * ye) / det,
				0.);
	 */
}
ImpIntersect imp_triangle_intersect_point(ImpTriangle * t,
										  ImpPoint * p,
										  ImpEdge ** eon) {

	ImpVertex *v1, *v2, *v3;
	ImpEdge *e1, *e2, *e3;

	ImpPoint *p1, *p2, *p3;

	ImpOrientation or1;
	ImpOrientation or2;
	ImpOrientation or3;

	_exit_if_fail(t || p || eon);

	imp_triangle_extract(t, NULL, &v1, &v2, &v3, &e1, &e2, &e3);

	*eon = NULL;

	p1 = IMP_POINT(v1);
	p2 = IMP_POINT(v2);
	p3 = IMP_POINT(v3);

	or1 = imp_point_orientation(p1, p2, p);
	if (or1 == IMP_ORIENTATION_IN_LEFT) {
		return IMP_INTERSECT_OUT;
	} else if (or1 == IMP_ORIENTATION_COLLINEAR) {
		if (imp_point_in_rect(p1, p2, p)) {
			/* is between */
			*eon = e1;
			return IMP_INTERSECT_ON;
		}
		return IMP_INTERSECT_OUT;
	}

	or2 = imp_point_orientation(p2, p3, p);
	if (or2 == IMP_ORIENTATION_IN_LEFT) {
		return IMP_INTERSECT_OUT;
	} else if (or2 == IMP_ORIENTATION_COLLINEAR) {
		if (imp_point_in_rect(p2, p3, p)) {
			/* is between */
			*eon = e2;
			return IMP_INTERSECT_ON;
		}
		return IMP_INTERSECT_OUT;
	}

	or3 = imp_point_orientation(p3, p1, p);
	if (or3 == IMP_ORIENTATION_IN_LEFT) {
		return IMP_INTERSECT_OUT;
	} else if (or3 == IMP_ORIENTATION_COLLINEAR) {
		if (imp_point_in_rect(p3, p1, p)) {
			/* is between */
			*eon = e3;
			return IMP_INTERSECT_ON;
		}
		return IMP_INTERSECT_OUT;
	}

	return IMP_INTERSECT_IN;
}
ImpIntersect imp_triangle_circle_intersect_point(ImpTriangle * t,
												 ImpPoint * p) {

	ImpPoint * p1, * p2, * p3;
	real or;

	_return_val_if_fail(p != NULL && t != NULL, 0.0);

	imp_triangle_extract_vertices(t,
								(ImpVertex **) & p1,
								(ImpVertex **) & p2,
								(ImpVertex **) & p3);

	or = _in_circle((real *) & p1->x,
					(real *) & p2->x,
					(real *) & p3->x,
					(real *) & p->x);

	if (or < 0) return IMP_INTERSECT_IN;
	else if (or > 0) return IMP_INTERSECT_OUT;
	else return IMP_INTERSECT_ON;
}
/* ========================================================================== */

void imp_triangle_print(ImpTriangle * t) {

	ImpVertex * v1;
	ImpVertex * v2;
	ImpVertex * v3;

	if (!t) {
		printf("[NULL]");
		return;
	}

	imp_triangle_extract_vertices(t, &v1, &v2, &v3);

	printf("{ ");
	imp_vertex_print(v1);
	printf("  ");
	imp_vertex_print(v2);
	printf("  ");
	imp_vertex_print(v3);
	printf(" }");

	//printf(" (p%li -> p%li -> p%li) ", v1->id, v2->id, v3->id);
}
boolean imp_triangle_is_ok(ImpTriangle * t) {
	boolean r1, r2, r3, r4, r5;
	ImpVertex *v1, *v2, *v3;
	ImpEdge *e1, *e2, *e3;

	imp_triangle_extract(t, NULL, &v1, &v2, &v3, &e1, &e2, &e3);
	if (!imp_segment_connect(IMP_SEGMENT(e1), v1, v2)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}
	if (!imp_segment_connect(IMP_SEGMENT(e2), v2, v3)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}
	if (!imp_segment_connect(IMP_SEGMENT(e3), v3, v1)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}

	imp_triangle_extract_vertices(t, &v1, &v2, &v3);
	if (v1 == v2) {
		return FALSE;
	}
	if (v2 == v3) {
		return FALSE;
	}
	if (v3 == v1) {
		return FALSE;
	}

	r1 = (t != NULL);
	r2 = (t->e[0] != NULL) && (t->e[1] != NULL) && (t->e[2] != NULL);
	r3 = imp_segment_are_connected(IMP_SEGMENT(t->e[0]), IMP_SEGMENT(t->e[1]));
	r4 = imp_segment_are_connected(IMP_SEGMENT(t->e[1]), IMP_SEGMENT(t->e[2]));
	r5 = imp_segment_are_connected(IMP_SEGMENT(t->e[2]), IMP_SEGMENT(t->e[0]));

	return r1 & r2 & r3 & r4 & r5;
}